'''
1.0.0 版本 
# Date: 2024/12/28
# Author: ZHENG
# Description:
    # 此版本功能如下：
        1. 加载pgm文件，并显示地图

'''

# -*- coding: utf-8 -*-
from PyQt5.QtWidgets import QApplication, QGraphicsView, QGraphicsScene, QGraphicsItem, QSizePolicy, QWidget
from PyQt5.QtGui import QColor, QPixmap, QImage, QPainter, QPen
from PyQt5.QtCore import QPointF, QRectF
from PyQt5.QtCore import Qt
import math
import numpy as np

pgmFile_path = "./maps/default.pgm"

class Vehicle(QGraphicsItem):
    
    def __init__(self, position=(0, 0), size=10):
        super().__init__()
        self.x, self.y = position
        self.size = size
        self.half_size = size // 2
        self.wheel_radius = size // 4
        self.head_size = size // 2
        self.setPos(self.x, self.y)

    def boundingRect(self):
        # 定义小车占据的空间范围
        adjust = 5
        return QRectF(-self.half_size - adjust, -self.size - adjust,
                      2 * (self.half_size + adjust), 2 * (self.size + adjust))

    def paint(self, painter, option, widget=None):
        # 绘制车身（矩形）
        body_rect = QRectF(-self.half_size, -self.size, self.size, self.size // 2)
        painter.setBrush(QColor('black'))
        painter.drawRect(body_rect)

        # 绘制左车轮
        left_wheel = QRectF(-self.half_size - self.wheel_radius, self.size // 4 - self.wheel_radius, 
                            2 * self.wheel_radius, 2 * self.wheel_radius)
        painter.drawEllipse(left_wheel)

        # 绘制右车轮
        right_wheel = QRectF(self.half_size - self.wheel_radius, self.size // 4 - self.wheel_radius, 
                             2 * self.wheel_radius, 2 * self.wheel_radius)
        painter.drawEllipse(right_wheel)

        # 绘制车头（三角形）
        points = [
            QPointF(-self.half_size, -self.head_size),
            QPointF(self.half_size, -self.head_size),
            QPointF(0, 0)
        ]
        painter.setBrush(QColor('red'))
        painter.drawPolygon(points)

    def set_position(self, pos):
        """设置小车的新位置"""
        self.x, self.y = pos
        self.setPos(self.x, self.y)

class DisplayCostMap(QWidget):
    def __init__(self, parent=None):
        super().__init__(parent)
        self.map_image = QImage()
        self.vehicle_pos = None  # (x, y, theta)
        self.map_info = {
            'resolution': 0.05,
            'origin_x': 0.0,
            'origin_y': 0.0
        }

    def set_map_image(self, qimage):
        self.map_image = qimage
        self.update()

    def set_vehicle_pos(self, x, y, theta):
        # 转换为像素坐标
        res = self.map_info['resolution']
        ox = self.map_info['origin_x']
        oy = self.map_info['origin_y']
        px = int((x - ox) / res)
        py = int((y - oy) / res)
        self.vehicle_pos = (px, py, theta)
    
    def paintEvent(self, event):
        """绘制地图和车辆位置"""
        painter = QPainter(self)
        painter.setRenderHint(QPainter.Antialiasing)
        
        # 绘制地图
        if self.map_image:
            # 缩放地图以适应控件
            scaled_img = self.map_image.scaled(
                self.width(), self.height(),
                Qt.KeepAspectRatio, Qt.SmoothTransformation
            )
            # 居中显示
            x_offset = (self.width() - scaled_img.width()) // 2
            y_offset = (self.height() - scaled_img.height()) // 2
            painter.drawImage(x_offset, y_offset, scaled_img)
            
            # 绘制车辆位置（转换为控件坐标）
            if self.map_info:
                map_x, map_y, theta = self.vehicle_pos
                
                # 转换到地图坐标
                scale_x = scaled_img.width() / self.map_info['width']
                scale_y = scaled_img.height() / self.map_info['height']
                
                # 计算控件中的位置
                widget_x = x_offset + (map_x - self.map_info['origin_x']) * scale_x
                widget_y = y_offset + (map_y - self.map_info['origin_y']) * scale_y
                
                # 绘制车辆
                painter.setPen(QPen(Qt.red, 2))
                painter.setBrush(Qt.red)
                size = 10
                painter.drawEllipse(int(widget_x - size/2), int(widget_y - size/2), size, size)
                
                # 绘制方向
                arrow_len = 15
                end_x = widget_x + arrow_len * np.cos(theta)
                end_y = widget_y + arrow_len * np.sin(theta)
                painter.drawLine(int(widget_x), int(widget_y), int(end_x), int(end_y))
        # painter = QPainter(self)
        
        # # 绘制地图
        # if not self.map_image.isNull():
        #     pixmap = QPixmap.fromImage(self.map_image)
        #     painter.drawPixmap(0, 0, self.width(), self.height(), pixmap)
        
        # # 绘制车辆位置
        # if self.vehicle_pos:
        #     px, py, theta = self.vehicle_pos
        #     size = 20
        #     # 绘制车身
        #     painter.setPen(Qt.red)
        #     painter.setBrush(Qt.NoBrush)
        #     painter.drawEllipse(px-5, py-5, 10, 10)
            
        #     # 绘制方向箭头
        #     arrow_len = 15
        #     dx = arrow_len * math.cos(theta)
        #     dy = arrow_len * math.sin(theta)
        #     painter.drawLine(px, py, px + dx, py + dy)

# class DisplayCostMap(QGraphicsView):
    
#     def __init__(self, parent=None):
#         super().__init__(parent)
#         self.scene = QGraphicsScene(self)
#         self.setScene(self.scene)
#         self.vehicle = None
#         self.map_image_item = None

#         # 设置尺寸策略为扩展
#         self.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Expanding)
        
#         # 确保滚动条总是关闭
#         self.setVerticalScrollBarPolicy(Qt.ScrollBarAlwaysOff)
#         self.setHorizontalScrollBarPolicy(Qt.ScrollBarAlwaysOff)

#     def update_data(self, image_path, vehicle_pos=None):
#         if image_path:
#             pixmap = QPixmap(image_path)
#             if pixmap.isNull():
#                 print(f"Failed to load image: {image_path}")
#                 return

#             self.map_image_item = self.scene.addPixmap(pixmap)
#             self.scene.setSceneRect(0.0, 0.0, pixmap.width(), pixmap.height())

#             # 移除旧车辆并添加新位置的车辆
#             if self.vehicle is not None:
#                 self.scene.removeItem(self.vehicle)
#             if vehicle_pos is not None:
#                 self.vehicle = Vehicle(position=vehicle_pos)
#                 self.scene.addItem(self.vehicle)

#             # 自动调整视图以适应场景
#             self.fitInView(self.scene.sceneRect(), Qt.KeepAspectRatio)
            
#             # 更新视图
#             self.update()

#     def set_vehicle_position(self, pos):
#         """设置小车的新位置"""
#         if self.vehicle is not None:
#             self.vehicle.set_position(pos)
#             self.update()

#     def resizeEvent(self, event):
#         super().resizeEvent(event)
#         if self.map_image_item:
#             self.fitInView(self.scene.sceneRect(), Qt.KeepAspectRatio)


# 示例用法
if __name__ == '__main__':
    import sys
    app = QApplication(sys.argv)
    
    display = DisplayCostMap()
    display.show()

    # 假设这里有一个PGM文件路径和车辆位置数据
    pgm_file_path = 'maps/default.pgm'
    vehicle_position = (100, 100)

    # 更新地图和车辆位置
    display.update_data(pgm_file_path, vehicle_position)

    sys.exit(app.exec_())


